
#include <Glut/glut.h>
#include <OpenGL/glu.h>

#include "display.h"

#include "rigidbody.h"
#include "rigidbodyengine.h"

#include "typedefs.h"
#include "angleaxis.h"

Display::Display(Scene *scene) {

    this->scene = scene;

    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glShadeModel(GL_SMOOTH);
//    glEnable(GL_LIGHTING);
//    glEnable(GL_LIGHT0);
//    glEnable(GL_MULTISAMPLE);
//    static GLfloat lightPosition[4] = { 0.5, 5.0, 7.0, 1.0 };
//    glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);

    displayMode = GL_FILL;
    showBBox = true;
    useMaterial = false;
}

void Display::draw(bool withNames)
{
    glPolygonMode(GL_FRONT_AND_BACK,displayMode);

    //std::cout << "draw" << std::endl;
    std::list<RigidBody*>::iterator it=scene->engine->bodies.begin();
    int i=0;

    GLfloat no_mat[] = { 0.0, 0.0, 0.0, 1.0 };
    GLfloat mat_diffuse[] = { 0.1, 0.5, 0.8, 1.0 };
    for(;it!=scene->engine->bodies.end();++it)
    {
        if(withNames)
            glPushName((*it)->id);

        QColor col = (*it)->color;

        qreal red = col.redF(), green = col.greenF(), blue = col.blueF();
        //col.getRgb(&r,&g,&b);
        if(!useMaterial)
            glColor3f(red,green,blue);
        else
        {
            mat_diffuse[0] = red; mat_diffuse[1] = green; mat_diffuse[2] = blue;
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, mat_diffuse);
            glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, mat_diffuse);
        }
        Math::Vector3 p = (*it)->position;
        Math::Quaternion q = (*it)->orientation;
        if((*it)->type == RigidBody::BOX)
        {
            drawBox((*it)->size,p,q);
        }
        else if((*it)->type == RigidBody::SPHERE)
        {
            drawSphere((*it)->radius,p);
        }
        else if((*it)->type == RigidBody::MESH)
        {
            drawMesh((*it)->mesh,p,q);
        }
        if(withNames)
            glPopName();
    }

    if(showBBox && scene->selectedBody)
    {
        glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
        glColor3f(1,0,0);
        //Math::Vector3 bbsize = (scene->selectedBody->aabb.max()-scene->selectedBody->aabb.min())*0.5;
        drawBox((scene->selectedBody->aabb.max()-scene->selectedBody->aabb.min())*0.5,scene->selectedBody->position,scene->selectedBody->orientation);

        glPolygonMode(GL_FRONT_AND_BACK,displayMode);
    }
}

void Display::drawBox(const Math::Vector3 &size, const Math::Vector3 &position, const Math::Quaternion &orientation)
{
    Math::Vector3 pmin = size*-1.0;
    Math::Vector3 pmax = size;
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    Math::Matrix33 rot = orientation.toRotationMatrix();
    Math::Real m[16];
    m[0] = rot(0,0); m[4] = rot(0,1); m[8] = rot(0,2);
    m[1] = rot(1,0); m[5] = rot(1,1); m[9] = rot(1,2);
    m[2] = rot(2,0); m[6] = rot(2,1); m[10] = rot(2,2);
    m[3] = 0;        m[7] = 0;        m[11] = 0;

    m[12] = position.x();
    m[13] = position.y();
    m[14] = position.z();
    m[15] = btScalar(1.0);
    glMultMatrixf(m);

    glBegin(GL_QUADS);									// Draw A Quad

        //glColor3f(0.0f,1.0f,0.0f);						// Set The Color To Green
        glVertex3f( pmax.x(), pmax.y(),pmin.z());					// Top Right Of The Quad (Top)
        glVertex3f( pmin.x(), pmax.y(),pmin.z());					// Top Left Of The Quad (Top)
        glVertex3f( pmin.x(), pmax.y(),pmax.z());					// Bottom Left Of The Quad (Top)
        glVertex3f( pmax.x(), pmax.y(),pmax.z());					// Bottom Right Of The Quad (Top)
        //glColor3f(1.0f5f,0.0f);						// Set The Color To Orange
        glVertex3f( pmax.x(), pmin.y(),pmax.z());					// Top Right Of The Quad (Bottom)
        glVertex3f( pmin.x(), pmin.y(),pmax.z());					// Top Left Of The Quad (Bottom)
        glVertex3f( pmin.x(), pmin.y(),pmin.z());				// Bottom Left Of The Quad (Bottom)
        glVertex3f( pmax.x(), pmin.y(),pmin.z());					// Bottom Right Of The Quad (Bottom)
        //glColor3f(1.0f0f,0.0f);						// Set The Color To Red
        glVertex3f( pmax.x(), pmax.y(),pmax.z());					// Top Right Of The Quad (Front)
        glVertex3f( pmin.x(), pmax.y(),pmax.z());					// Top Left Of The Quad (Front)
        glVertex3f( pmin.x(), pmin.y(),pmax.z());					// Bottom Left Of The Quad (Front)
        glVertex3f( pmax.x(), pmin.y(),pmax.z());					// Bottom Right Of The Quad (Front)
        //glColor3f(1.0f0f,0.0f);						// Set The Color To Yellow
        glVertex3f( pmax.x(), pmin.y(),pmin.z());					// Top Right Of The Quad (Back)
        glVertex3f( pmin.x(), pmin.y(),pmin.z());				// Top Left Of The Quad (Back)
        glVertex3f( pmin.x(), pmax.y(),pmin.z());					// Bottom Left Of The Quad (Back)
        glVertex3f( pmax.x(), pmax.y(),pmin.z());					// Bottom Right Of The Quad (Back)
        //glColor3f(0.0f0f,1.0f);						// Set The Color To Blue
        glVertex3f( pmin.x(), pmax.y(),pmax.z());					// Top Right Of The Quad (Left)
        glVertex3f( pmin.x(), pmax.y(),pmin.z());					// Top Left Of The Quad (Left)
        glVertex3f( pmin.x(), pmin.y(),pmin.z());				// Bottom Left Of The Quad (Left)
        glVertex3f( pmin.x(), pmin.y(),pmax.z());					// Bottom Right Of The Quad (Left)
        //glColor3f(1.0f0f,1.0f);						// Set The Color To Violet
        glVertex3f( pmax.x(), pmax.y(),pmin.z());					// Top Right Of The Quad (Right)
        glVertex3f( pmax.x(), pmax.y(),pmax.z());					// Top Left Of The Quad (Right)
        glVertex3f( pmax.x(), pmin.y(),pmax.z());					// Bottom Left Of The Quad (Right)
        glVertex3f( pmax.x(), pmin.y(),pmin.z());					// Bottom Right Of The Quad (Right)
    glEnd();											// Done Drawing The Quad
    glPopMatrix();
}

void Display::drawSphere(const Math::Real &radius, const Math::Vector3 &position)
{
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glTranslatef(position.x(),position.y(),position.z());
    glutSolidSphere(radius,10,10);
    glPopMatrix();
}

void Display::drawMesh( TriMesh *mesh, const Math::Vector3 &position, const Math::Quaternion &orientation)
{
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    Math::Matrix33 rot = orientation.toRotationMatrix();
    Math::Real m[16];
    m[0] = rot(0,0); m[4] = rot(0,1); m[8] = rot(0,2);
    m[1] = rot(1,0); m[5] = rot(1,1); m[9] = rot(1,2);
    m[2] = rot(2,0); m[6] = rot(2,1); m[10] = rot(2,2);
    m[3] = 0;        m[7] = 0;        m[11] = 0;

    m[12] = position.x();
    m[13] = position.y();
    m[14] = position.z();
    m[15] = btScalar(1.0);
    glMultMatrixf(m);

    glBegin(GL_TRIANGLES);
    for(int i=0;i<mesh->Faces().size();++i)
    {
        Vector3 v1 = mesh->Faces()[i]->Vertex1()->Position();
        Vector3 v2 = mesh->Faces()[i]->Vertex2()->Position();
        Vector3 v3 = mesh->Faces()[i]->Vertex3()->Position();
        glVertex3f(v1.x(),v1.y(),v1.z());
        glVertex3f(v2.x(),v2.y(),v2.z());
        glVertex3f(v3.x(),v3.y(),v3.z());
    }
    glEnd();

    glPopMatrix();
}
